#include "sbus.h"

uint16_t sbus_channel_info[16] = {0};
uint8_t sbus_recv_data = 0;
uint8_t sbus_buf[32];
uint16_t sbus_recv_cnt = 0;
SBUS_BufferTypeDef sbus_info;
uint32_t sbus_keepalive_cnt = 1000;

void SBUS_Init(void)
{
	SBUS_StartRecv(&sbus_recv_data);
}

void SBUS_RecvCallback(void)
{
	if( sbus_recv_cnt )
	{
		sbus_buf[sbus_recv_cnt] = sbus_recv_data;
		sbus_recv_cnt++;
		if(sbus_recv_cnt>=25) 
		{
			SBUS_KeepAliveClear();
			sbus_recv_cnt = 0;
			memcpy((uint8_t*)&sbus_info,sbus_buf,25);
			SBUS_Unpack(&sbus_info,sbus_channel_info);
			//Analyse_SBUS(&rc_sbus_buffer);  
		}
	}
	else if( sbus_recv_data == 0x0f )
	{
		sbus_buf[sbus_recv_cnt] = sbus_recv_data;
		sbus_recv_cnt++;
	}				
	SBUS_StartRecv(&sbus_recv_data);
}

void SBUS_Unpack(void *sbus,uint16_t *channel_info)
{
	SBUS_BufferTypeDef *buf = sbus;

	//if(buf->flag == 0x10)			//MCR6旧遥控
	//if(buf->flag == 0x0C)		//WFLY遥控
	if(buf->flag == 0x08)		//新遥控
	{
		sbus_keepalive_cnt = 1000;
		return;
	}	
	channel_info[0] = buf->data1;
	channel_info[1] = buf->data2_L | (buf->data2_H)<<5;
	channel_info[2] = buf->data3_L | (buf->data3_H)<<10;
	channel_info[3] = buf->data4;
	channel_info[4] = buf->data5_L | (buf->data5_H)<<4;
	channel_info[5] = buf->data6_L | (buf->data6_H)<<9;
	channel_info[6] = buf->data7;
	channel_info[7] = buf->data8_L | (buf->data8_H)<<3;
	channel_info[8] = buf->data9_L | (buf->data9_H)<<8;	
	channel_info[9] = buf->data10;
	channel_info[10] = buf->data11_L | (buf->data11_H)<<2;
	channel_info[11] = buf->data12_L | (buf->data12_H)<<7;		
	channel_info[12] = buf->data13;
	channel_info[13] = buf->data14_L | (buf->data14_H)<<1;
	channel_info[14] = buf->data15_L | (buf->data15_H)<<6;		
	channel_info[15] = buf->data16;	
}

void SBUS_GetChannelValue(uint16_t *channel) 
{
	uint8_t i;
	for(i=0;i<10;i++)
	{
		channel[i] = sbus_channel_info[i];
	}
}



__WEAK void SBUS_StartRecv(uint8_t *data)
{
	UNUSED(data);
}


void SBUS_KeepAliveClear(void)
{
	sbus_keepalive_cnt = 0;
}

uint8_t SBUS_KeepAliveTask(uint32_t period)
{
	sbus_keepalive_cnt += period;
	
	if( sbus_keepalive_cnt >= 1000 )
	{
		sbus_keepalive_cnt = 1000;
		return 0xff;
	}
	return 0;
}






